import math
import random


# ---------------- 参数 ----------------
N = 5                      # 飞机数量
R_MIN = 300.0              # 最小转弯半径 /m
T_LOITER = 40.0            # 盘旋周期 /s
V_LOITER = 2 * math.pi * R_MIN / T_LOITER  # 盘旋速度
DT = 0.05                  # 仿真步长 20 Hz
SCALE = 0.8                # 绘图缩放（像素/米）
CENTER_X = 600             # 绘图中心像素
CENTER_Y = 400

# 前 5 s 每架机的盘旋中心（随机 500×500 区域）
loiter_c = [(-250 + random.random() * 500,
             -250 + random.random() * 500) for _ in range(N)]
loiter_phi0 = [random.random() * 2 * math.pi for _ in range(N)]  # 初始相位

# ---------------- 飞机类 ----------------
class Aircraft:
    def __init__(self, idx):
        self.idx = idx
        self.t = 0.0
        self.phase = 0  # 0 盘旋，1 转场
        self.path = []  # 航路点
        self.L = 0.0    # 航路总长
        self.v = V_LOITER
        self.s = 0.0    # 已飞距离
        self.x, self.y = 0.0, 0.0
        self.phi = 0.0  # 航向
        self.target = (0.0, 0.0)

    def loiter_step(self, dt):
        w = 2 * math.pi / T_LOITER
        phi = self.t * w + loiter_phi0[self.idx]
        # 前半周期右转，后半周期左转
        if phi < math.pi:
            # 右转半圆
            ang = phi
            c = (loiter_c[self.idx][0] + R_MIN, loiter_c[self.idx][1])
            self.x = c[0] + R_MIN * math.cos(ang + math.pi)
            self.y = c[1] + R_MIN * math.sin(ang + math.pi)
            self.phi = ang + math.pi / 2
        else:
            # 左转半圆
            ang = phi - math.pi
            c = (loiter_c[self.idx][0] - R_MIN, loiter_c[self.idx][1])
            self.x = c[0] + R_MIN * math.cos(-ang)
            self.y = c[1] + R_MIN * math.sin(-ang)
            self.phi = -ang - math.pi / 2

    def follow_path_step(self, dt):
        if not self.path:
            return
        v = self.v
        self.s += v * dt
        acc = 0.0
        for i in range(len(self.path) - 1):
            a = self.path[i]
            b = self.path[i + 1]
            seg_len = math.hypot(b[0] - a[0], b[1] - a[1])
            if acc + seg_len >= self.s:
                # 在本段
                t = (self.s - acc) / seg_len
                self.x = a[0] + t * (b[0] - a[0])
                self.y = a[1] + t * (b[1] - a[1])
                self.phi = math.atan2(b[1] - a[1], b[0] - a[0])
                return
            acc += seg_len
        # 到终点
        self.x, self.y = self.path[-1]
        self.phase = 2  # 到达

    def step(self, dt):
        self.t += dt
        if self.phase == 0:
            self.loiter_step(dt)
        elif self.phase == 1:
            self.follow_path_step(dt)
            
if __name__ == '__main__':
    aircrafts = [Aircraft(i) for i in range(N)]
    for t in range(5000):
        ac=aircrafts[0]
        if ac.phase == 1:
            print('开始')
        if ac.phase == 2:
            print('到达')
        ac.step(DT)